/*
 * ============================================================================
 * GNU General Public License
 * ============================================================================
 *
 * Copyright (C) 2015 Infinite Automation Software. All rights reserved.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * When signing a commercial license with Infinite Automation Software,
 * the following extension to GPL is made. A special exception to the GPL is
 * included to allow you to distribute a combined work that includes BAcnet4J
 * without being obliged to provide the source code for any proprietary components.
 *
 * See www.infiniteautomation.com for commercial license options.
 *
 * @author Matthew Lohbihler
 */
package org.thanos.bacnet.npdu.ip;

import org.thanos.bacnet.enums.MaxApduLength;
import org.thanos.bacnet.exception.BACnetException;
import org.thanos.bacnet.npdu.MessageValidationException;
import org.thanos.bacnet.npdu.NPDU;
import org.thanos.bacnet.npdu.Network;
import org.thanos.bacnet.npdu.NetworkIdentifier;
import org.thanos.bacnet.transport.Transport;
import org.thanos.bacnet.type.constructed.Address;
import org.thanos.bacnet.type.primitive.OctetString;
import org.thanos.bacnet.util.BACnetUtils;
import org.thanos.bacnet.util.sero.ByteQueue;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import java.io.IOException;
import java.net.DatagramPacket;
import java.net.DatagramSocket;
import java.net.InetAddress;
import java.net.InetSocketAddress;
import java.net.NetworkInterface;
import java.net.SocketException;
import java.net.UnknownHostException;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.List;
import java.util.concurrent.CopyOnWriteArrayList;
import java.util.concurrent.ScheduledFuture;
import java.util.concurrent.TimeUnit;
import java.util.concurrent.atomic.AtomicBoolean;

/**
 * Use IpNetworkBuilder to create.
 */
public class IpNetwork extends Network implements Runnable {
	private static final Logger LOG = LoggerFactory.getLogger(IpNetwork.class);
	public static final byte BVLC_TYPE = (byte) 0x81;
	public static final int DEFAULT_PORT = 0xBAC0; // == 47808
	public static final String DEFAULT_BIND_IP = "0.0.0.0";

	private static final int MESSAGE_LENGTH = 2048;

	private final int port;
	private final String localBindAddressStr;
	private final String broadcastAddressStr;
	private final String subnetMaskStr;
	private final boolean reuseAddress;

	// BBMD support
	private List<BDTEntry> broadcastDistributionTable = new ArrayList<>();
	final List<FDTEntry> foreignDeviceTable = new CopyOnWriteArrayList<>();
	private ScheduledFuture<?> ftdMaintenance;
	private final AtomicBoolean bbmdEnabled = new AtomicBoolean(false);

	// When acting as a foreign device...
	final Object foreignBBMDLock = new Object();
	InetSocketAddress foreignBBMD;
	private int foreignTTL;
	private int bbmdResponse;
	private ScheduledFuture<?> foreignRegistrationMaintenance;

	// Runtime
	private Thread thread;
	private DatagramSocket socket;
	private OctetString broadcastMAC;
	private InetSocketAddress localBindAddress;
	private byte[] subnetMask;
	private long bytesOut;
	private long bytesIn;

	/**
	 * Use an IpNetworkBuilder to create instances.
	 */
	IpNetwork(final int port, final String localBindAddress, final String broadcastAddress, final String subnetMask,
	          final int localNetworkNumber, final boolean reuseAddress) {
		super(localNetworkNumber);
		this.port = port;
		this.localBindAddressStr = localBindAddress;
		this.broadcastAddressStr = broadcastAddress;
		this.subnetMaskStr = subnetMask;
		this.reuseAddress = reuseAddress;
	}

	@Override
	public NetworkIdentifier getNetworkIdentifier() {
		return new IpNetworkIdentifier(port, localBindAddressStr);
	}

	@Override
	public MaxApduLength getMaxApduLength() {
		return MaxApduLength.UP_TO_1476;
	}

	public int getPort() {
		return port;
	}

	public InetSocketAddress getLocalBindAddress() {
		return localBindAddress;
	}

	public String getBroadcastAddresss() {
		return broadcastAddressStr;
	}

	@Override
	public long getBytesOut() {
		return bytesOut;
	}

	@Override
	public long getBytesIn() {
		return bytesIn;
	}

	/**
	 * Get the network socket, useful for routing purposes
	 *
	 * @return
	 */
	public DatagramSocket getSocket() {
		return socket;
	}

	@Override
	public void initialize(final Transport transport) throws Exception {
		super.initialize(transport);
		localBindAddress = InetAddrCache.get(localBindAddressStr, port);

		if (reuseAddress) {
			socket = new DatagramSocket(null);
			socket.setReuseAddress(true);
			if (!socket.getReuseAddress())
				LOG.warn("reuseAddress was set, but not supported by the underlying platform");
			socket.bind(localBindAddress);
		} else
			socket = new DatagramSocket(localBindAddress);
		socket.setBroadcast(true);
		socket.setReceiveBufferSize(1024 * 1024 * 128);
		//        broadcastAddress = new Address(broadcastIp, port, new Network(0xffff, new byte[0]));
		broadcastMAC = IpNetworkUtils.toOctetString(broadcastAddressStr, port);
		subnetMask = BACnetUtils.dottedStringToBytes(subnetMaskStr);

		thread = new Thread(this, "Thanos BACnet IP socket listener for " + transport.getLocalDevice().getId());
		thread.start();
	}

	@Override
	public void terminate() {
		unregisterAsForeignDevice();
		if (socket != null)
			socket.close();
		if (ftdMaintenance != null)
			ftdMaintenance.cancel(false);
	}

	@Override
	protected OctetString getBroadcastMAC() {
		return broadcastMAC;
	}

	public Address getBroadcastAddress(final int port) {
		return IpNetworkUtils.toAddress(broadcastAddressStr, port);
	}

	/**
	 * Attempts an initial registration of this device as a foreign device in a BBMD. This method blocks until the
	 * response (ACK or NAK) is received, or a timeout occurs (no response).
	 * <p>
	 * If the device is successfully registered (ACK), regular re-registration requests will be sent to maintain the
	 * registration. If any of these requests fail, the local device's exception dispatcher will be notified.
	 * <p>
	 * If the device is to be un-registered, the registerAsForeignDevice method should be called. This will be done
	 * automatically if the local device is terminated.
	 *
	 * @param addr       The address of the BBMD where our device wants to be registered
	 * @param timeToLive The time until we are automatically removed out of the FDT
	 * @throws BACnetException if a timeout occurs, a NAK is received, the device is already registered as a foreign device, or
	 *                         the request could not be sent.
	 */
	public void registerAsForeignDevice(final InetSocketAddress addr, final int timeToLive) throws BACnetException {
		if (timeToLive < 1)
			throw new IllegalArgumentException("timeToLive cannot be less than 1");
		if (getTransport() == null || getTransport().getLocalDevice() == null)
			throw new IllegalArgumentException(
					"Network must be used within a local device before foreign device registration can be performed.");

		synchronized (foreignBBMDLock) {
			if (foreignBBMD != null)
				throw new BACnetException("Already registered as a foreign device");

			foreignBBMD = addr;
			foreignTTL = timeToLive;

			try {
				sendForeignDeviceRegistration();
			} catch (final BACnetException e) {
				foreignBBMD = null;
				throw e;
			}

			// No exception occurred, so create a task that will re-register this device at appropriate intervals.
			// Including the grace period, this should result in rereg 1 minute before expiration.
			int interval = timeToLive - 30;
			// ... but since the ttl can be less than 30, we need to correct.
			if (interval < 15)
				// Minimum of 15s.
				interval = 15;
			foreignRegistrationMaintenance = getTransport().getLocalDevice().scheduleAtFixedRate(() -> {
				synchronized (foreignBBMDLock) {
					// Check just in case...
					if (foreignBBMD == null)
						return;

					try {
						sendForeignDeviceRegistration();
					} catch (final BACnetException e) {
						getTransport().getLocalDevice().getExceptionDispatcher().fireReceivedException(e);
					}
				}
			}, interval, interval, TimeUnit.SECONDS);
		}
	}

	/**
	 * Requests that this device be un-registered at the location at which it was previously registered. Quietly
	 * ignores the case where it is not already registered.
	 */
	public void unregisterAsForeignDevice() {
		synchronized (foreignBBMDLock) {
			if (foreignBBMD != null) {
				try {
					deleteForeignDeviceTableEntry(foreignBBMD, localBindAddress);
				} catch (final BACnetException e) {
					LOG.info("Device de-registration failed", e);
				}
			}
			if (foreignRegistrationMaintenance != null)
				foreignRegistrationMaintenance.cancel(false);
			foreignBBMD = null;
			foreignRegistrationMaintenance = null;
		}
	}

	@Override
	public void sendNPDU(final Address recipient, final OctetString router, final ByteQueue npdu,
	                     final boolean broadcast, final boolean expectsReply) throws BACnetException {
		final ByteQueue queue = new ByteQueue();

		// BACnet virtual link layer detail
		queue.push(BVLC_TYPE);

		InetSocketAddress addr = foreignBBMD;
		if (addr != null && broadcast) {
			// Distribute-Broadcast-To-Network. This device is registered as a foreign device in a BBMD, so send the
			// message as a distribute broadcast to network.
			queue.push(9);
		} else {
			// Original-Unicast-NPDU, or Original-Broadcast-NPDU
			queue.push(broadcast ? 0xb : 0xa);

			final OctetString dest = getDestination(recipient, router);
			addr = IpNetworkUtils.getInetSocketAddress(dest);
		}

		queue.pushU2B(npdu.size() + 4);

		// Combine the queues
		queue.push(npdu);

		sendPacket(addr, queue.popAll());
	}

	private void sendPacket(final InetSocketAddress addr, final byte[] data) throws BACnetException {
		try {
			final DatagramPacket packet = new DatagramPacket(data, data.length, addr);
			socket.send(packet);
			bytesOut += data.length;
		} catch (final Exception e) {
			throw new BACnetException(e);
		}
	}

	//
	// For receiving
	@Override
	public void run() {
		final byte[] buffer = new byte[MESSAGE_LENGTH];
		final DatagramPacket p = new DatagramPacket(buffer, buffer.length);

		while (!socket.isClosed()) {
			try {
					socket.receive(p);
					bytesIn += p.getLength();
					// Create a new byte queue for the message, because the queue will probably be processed in the
					// transport thread.
					final ByteQueue queue = new ByteQueue(p.getData(), 0, p.getLength());
					final OctetString link = IpNetworkUtils.toOctetString(p.getAddress().getAddress(), p.getPort());
					//在这里监听广播消息
					handleIncomingData(queue, link);
					// Reset the packet.
					p.setData(buffer);
			} catch (@SuppressWarnings("unused") final IOException e) {
				// no op. This happens if the socket gets closed by the destroy method.
				LOG.error(e.getMessage(), e);
			}
		}
	}

	@Override
	protected NPDU handleIncomingDataImpl(final ByteQueue queue, final OctetString linkService) throws Exception {
		LOG.trace("Received request from {}", linkService);

		// Initial parsing of IP message.
		// BACnet/IP
		if (queue.pop() != BVLC_TYPE)
			throw new MessageValidationException("Protocol id is not BACnet/IP (0x81)");

		final byte function = queue.pop();

		final int length = BACnetUtils.popShort(queue);
		if (length != queue.size() + 4)
			throw new MessageValidationException(
					"Length field does not match data: given=" + length + ", expected=" + (queue.size() + 4));

		NPDU npdu = null;
		if (function == 0x0) {
			final int result = BACnetUtils.popShort(queue);

			if (result == 0x10)
				LOG.error("Write-Broadcast-Distrubution-Table failed!");
			else if (result == 0x20)
				LOG.error("Read-Broadcast-Distrubution-Table failed!");
			else if (result == 0x30)
				LOG.error("Register-Foreign-Device failed!");
			else if (result == 0x40)
				LOG.error("Read-Foreign-Device-Table failed!");
			else if (result == 0x50)
				LOG.error("Delete-Foreign-Device-Table-Entry failed!");
			else if (result == 0x60)
				LOG.error("Distribute-Broadcast-To-Network failed!");
			else if (result != 0)
				LOG.warn("Received unexpected BVLC result: " + result);

			// Response management.
			bbmdResponse = result;
			final InetSocketAddress addr = foreignBBMD;
			if (addr != null) {
				synchronized (addr) {
					addr.notifyAll();
				}
			}
		} else if (function == 0x1)
			// Write-Broadcast-Distribution-Table
			writeBDT(queue, linkService);
		else if (function == 0x2)
			// Read-Broadcast-Distribution-Table
			readBDT(linkService);
		else if (function == 0x3)
			// Not implemented because this does not send Read-Broadcast-Distribution-Table requests, and so should
			// not receive any responses.
			throw new BACnetException("Read-Broadcast-Distribution-Table-Ack not implemented");
		else if (function == 0x4) {
			// Forwarded-NPDU.
			// If this is a BBMD, it could forward this message to its local broadcast address, meaning it could
			// receive its own forwards. Check if the link service is itself, and ignore if so.
			if (!linkService.equals(IpNetworkUtils.toOctetString(localBindAddress))) {
				forwardNPDU(queue, linkService);

				// Process the NPDU locally
				final byte[] address = new byte[6];
				queue.pop(address);
				final OctetString origin = new OctetString(address);
				npdu = parseNpduData(queue, origin);
			}
		} else if (function == 0x5)
			// Register-Foreign-Device
			registerForeignDevice(queue, linkService);
		else if (function == 0x6)
			// Read-Foreign-Device-Table
			readFDT(linkService);
		else if (function == 0x7)
			// Not implemented because this does not send Read-Foreign-Device-Table requests, and so should
			// not receive any responses.
			throw new BACnetException("Read-Foreign-Device-Table-Ack not implemented");
		else if (function == 0x8)
			// Delete-Foreign-Device-Table-Entry
			deleteFDT(queue, linkService);
		else if (function == 0x9) {
			// Distribute-Broadcast-To-Network
			final boolean ok = distributeBroadcastToNetwork(queue, linkService);
			if (ok)
				// Only process locally if the foreign device is valid.
				npdu = parseNpduData(queue, linkService);
		} else if (function == 0xa)
			// Original-Unicast-NPDU
			npdu = parseNpduData(queue, linkService);
		else if (function == 0xb) {
			// Original-Broadcast-NPDU
			originalBroadcast(queue, linkService);

			npdu = parseNpduData(queue, linkService);
		} else
			throw new MessageValidationException(
					"Unhandled BVLC function type: 0x" + Integer.toHexString(function & 0xff));

		return npdu;
	}

	//
	//
	// Convenience methods
	//
	public Address getAddress(final InetAddress inetAddress) {
		try {
			return IpNetworkUtils.toAddress(getLocalNetworkNumber(), inetAddress.getAddress(), port);
		} catch (final Exception e) {
			// Should never happen, so just wrap in a RuntimeException
			throw new RuntimeException(e);
		}
	}

	public static InetAddress getDefaultLocalInetAddress() throws UnknownHostException, SocketException {
		for (final NetworkInterface iface : Collections.list(NetworkInterface.getNetworkInterfaces())) {
			for (final InetAddress addr : Collections.list(iface.getInetAddresses())) {
				if (!addr.isLoopbackAddress() && addr.isSiteLocalAddress())
					return addr;
			}
		}

		return InetAddress.getLocalHost();
	}

	@Override
	public Address[] getAllLocalAddresses() {
		try {
			final ArrayList<Address> result = new ArrayList<>();
			for (final NetworkInterface iface : Collections.list(NetworkInterface.getNetworkInterfaces())) {
				for (final InetAddress addr : Collections.list(iface.getInetAddresses())) {
					if (!addr.isLoopbackAddress() && addr.isSiteLocalAddress())
						result.add(getAddress(addr));
				}
			}

			// Check if the configured local bind address is one of the addresses. If not, add it. This is
			// necessary if the local bind address is a loopback.
			final Address config = IpNetworkUtils.toAddress(localBindAddress);
			boolean found = false;
			for (final Address addr : result) {
				if (addr.equals(config)) {
					found = true;
					break;
				}
			}

			if (!found)
				result.add(config);

			return result.toArray(new Address[result.size()]);
		} catch (final Exception e) {
			// Should never happen, so just wrap in a RuntimeException
			throw new RuntimeException(e);
		}
	}

	@Override
	public Address getLoopbackAddress() {
		return IpNetworkUtils.toAddress("127.0.0.1", DEFAULT_PORT);
	}

	@Override
	public int hashCode() {
		final int prime = 31;
		int result = super.hashCode();
		result = prime * result + (broadcastAddressStr == null ? 0 : broadcastAddressStr.hashCode());
		result = prime * result + (localBindAddressStr == null ? 0 : localBindAddressStr.hashCode());
		result = prime * result + port;
		return result;
	}

	@Override
	public boolean equals(final Object obj) {
		if (this == obj)
			return true;
		if (!super.equals(obj))
			return false;
		if (getClass() != obj.getClass())
			return false;
		final IpNetwork other = (IpNetwork) obj;
		if (broadcastAddressStr == null) {
			if (other.broadcastAddressStr != null)
				return false;
		} else if (!broadcastAddressStr.equals(other.broadcastAddressStr))
			return false;
		if (localBindAddressStr == null) {
			if (other.localBindAddressStr != null)
				return false;
		} else if (!localBindAddressStr.equals(other.localBindAddressStr))
			return false;
		if (port != other.port)
			return false;
		return true;
	}

	//
	//
	// BBMD
	//
	static class BDTEntry {
		// The IP address of the other BBMD
		byte[] address;
		int port;

		// If messages are to be distributed on the remote IP subnet using directed broadcasts, the broadcast
		// distribution mask shall be identical to the subnet mask associated with the subnet, i.e., all 1's in the
		// network portion of the 4-octet IP address field and all 0's in the host portion. If messages are to be
		// distributed on the remote IP subnet by sending the message directly to the remote BBMD, the broadcast
		// distribution mask shall be all 1's. The broadcast distribution masks referring to the same IP subnet shall
		// be identical in each BDT.
		byte[] distributionMask;
	}

	static class FDTEntry {
		InetSocketAddress address;
		int timeToLive;
		long endTime;
	}

	private void writeBDT(final ByteQueue queue, final OctetString origin) throws BACnetException {
		final ByteQueue response = new ByteQueue();
		response.push(BVLC_TYPE);
		response.push(0); // Result
		response.pushU2B(6); // Length

		if (bbmdEnabled.get()) {
			try {
				final List<BDTEntry> list = new ArrayList<>();

				while (queue.size() > 0) {
					final BDTEntry e = new BDTEntry();
					e.address = new byte[4];
					queue.pop(e.address);
					e.port = queue.popU2B();
					e.distributionMask = new byte[4];
					queue.pop(e.distributionMask);
					list.add(e);
				}

				// Successfully read. Replace the current BDT.
				broadcastDistributionTable = list;

				response.pushU2B(0); // Ok
			} catch (final Exception e) {
				LOG.error("BDT write failed", e);
				response.pushU2B(0x10); // NAK
			}
		} else {
			response.pushU2B(0x10); // NAK
		}
		sendPacket(IpNetworkUtils.getInetSocketAddress(origin), response.popAll());
	}

	private void readBDT(final OctetString origin) throws BACnetException {
		final ByteQueue response = new ByteQueue();
		response.push(BVLC_TYPE);
		if (bbmdEnabled.get()) {
			try {
				final ByteQueue list = new ByteQueue();

				for (final BDTEntry e : broadcastDistributionTable) {
					list.push(e.address);
					list.pushU2B(e.port);
					list.push(e.distributionMask);
				}

				// Successfully written.
				response.push(3); // Read-Broadcast-Distribution-Table-Ack
				response.pushU2B(4 + list.size()); // Length
				response.push(list); // List
			} catch (final Exception e) {
				LOG.error("BDT read failed", e);
				response.push(0); // Result
				response.pushU2B(6); // Length
				response.pushU2B(0x20); // NAK
			}
		} else {
			response.push(0); // Result
			response.pushU2B(6); // Length
			response.pushU2B(0x20); // NAK
		}
		sendPacket(IpNetworkUtils.getInetSocketAddress(origin), response.popAll());
	}

	private void forwardNPDU(final ByteQueue partial, final OctetString origin) throws BACnetException {
		// Determine whether to the message should be broadcast locally.
		boolean doLocalBroadcast = !broadcastDistributionTable.isEmpty();

		if (doLocalBroadcast) {
			// 1) If the origin is on the same subnet, do not broadcast locally.
			boolean fromSameSubnet = true;
			for (int i = 0; i < 4; i++) {
				final int b1 = localBindAddress.getAddress().getAddress()[i] & subnetMask[i];
				final int b2 = origin.getBytes()[i] & subnetMask[i];
				if (b1 != b2) {
					fromSameSubnet = false;
					break;
				}
			}

			if (fromSameSubnet)
				doLocalBroadcast = false;
		}

		if (doLocalBroadcast) {
			// 2) If the mask of the BDT entry for this BBMD is not all 1s, do not broadcast locally.
			final byte[] myAddress = localBindAddress.getAddress().getAddress();

			// Find this BDT entry.
			BDTEntry thisEntry = null;
			for (final BDTEntry e : broadcastDistributionTable) {
				if (Arrays.equals(e.address, myAddress)) {
					thisEntry = e;
					break;
				}
			}

			if (thisEntry == null) {
				// Not found. This is a configuration problem. Don't broadcast.
				LOG.warn("Configuration error: could not find BDT entry for this instance.");
				doLocalBroadcast = false;
			} else {
				// We only need to check the last byte (the last bit in actually), because any zeros all need to
				// be on the right.
				if (thisEntry.distributionMask[3] != (byte) 255)
					doLocalBroadcast = false;
			}
		}

		// Check if anything needs to be done.
		if (foreignDeviceTable.isEmpty() && !doLocalBroadcast)
			return;

		// The BVLC type, function and length were removed from this queue, so recreate.
		final ByteQueue fwd = new ByteQueue();
		fwd.push(BVLC_TYPE);
		fwd.push(4); // Forward
		fwd.pushU2B(4 + partial.size()); // Length
		fwd.push(partial);
		final byte[] toSend = fwd.popAll();

		if (doLocalBroadcast)
			sendPacket(InetAddrCache.get(broadcastAddressStr, port), toSend);

		// Forward to all foreign devices.
		for (final FDTEntry fd : foreignDeviceTable)
			sendPacket(fd.address, toSend);
	}

	private void originalBroadcast(final ByteQueue partial, final OctetString originStr) throws BACnetException {
		// Check if anything needs to be done.
		if (foreignDeviceTable.isEmpty() && broadcastDistributionTable.isEmpty())
			return;

		final ByteQueue fwd = new ByteQueue();
		fwd.push(BVLC_TYPE);
		fwd.push(4); // Forward
		fwd.pushU2B(10 + partial.size()); // Length
		fwd.push(originStr.getBytes()); // Origin
		fwd.push(partial);
		final byte[] toSend = fwd.popAll();

		try {
			final byte[] myAddress = localBindAddress.getAddress().getAddress();

			// Send to all subnets except own
			for (final BDTEntry e : broadcastDistributionTable) {
				if (Arrays.equals(e.address, myAddress))
					continue;
				sendToBDT(e, toSend);
			}

			// Forward to all foreign devices.
			for (final FDTEntry fd : foreignDeviceTable)
				sendPacket(fd.address, toSend);
		} catch (final UnknownHostException e) {
			throw new BACnetException(e);
		}
	}

	private void registerForeignDevice(final ByteQueue queue, final OctetString originStr) throws BACnetException {
		final InetSocketAddress origin = IpNetworkUtils.getInetSocketAddress(originStr);
		LOG.debug("Recieved registerForeignDevice request from {}", origin);

		final ByteQueue response = new ByteQueue();
		response.push(BVLC_TYPE);
		response.push(0); // Response type
		response.pushU2B(6); // Length

		if (bbmdEnabled.get()) {
			final int timeToLive = queue.popU2B();
			if (timeToLive < 1) {
				response.pushU2B(0x30); // NAK
			} else {
				// Check if the device is already in the list. If so, update its start time. Otherwise, add it.
				FDTEntry fd = null;
				synchronized (foreignDeviceTable) {
					for (final FDTEntry e : foreignDeviceTable) {
						if (e.address.equals(origin)) {
							fd = e;
							break;
						}
					}

					if (fd == null) {
						// Add the FDT entry
						fd = new FDTEntry();
						fd.address = origin;
						foreignDeviceTable.add(fd);

						if (ftdMaintenance == null) {
							// Add a job to expire foreign device registrations.
							ftdMaintenance = getTransport().getLocalDevice().scheduleAtFixedRate(() -> {
								final long now = getTransport().getLocalDevice().getClock().millis();

								synchronized (foreignDeviceTable) {
									final List<FDTEntry> toRemove = new ArrayList<>();

									for (final FDTEntry e : foreignDeviceTable) {
										if (e.endTime < now) {
											LOG.debug("Removing expired foreign device: " + e);
											toRemove.add(e);
										}
									}

									if (!toRemove.isEmpty()) {
										foreignDeviceTable.removeAll(toRemove);
									}
								}
							}, 10, 10, TimeUnit.SECONDS);
						}
					}
				}

				fd.timeToLive = timeToLive;
				fd.endTime = getTransport().getLocalDevice().getClock().millis() + (timeToLive + 30) * 1000; // Adds a 30-second grace period, as per J.5.2.3

				response.pushU2B(0); // Success
			}
		} else {
			response.pushU2B(0x30); // NAK
		}
		sendPacket(origin, response.popAll());
	}

	private void readFDT(final OctetString origin) throws BACnetException {
		final ByteQueue response = new ByteQueue();
		response.push(BVLC_TYPE);

		if (bbmdEnabled.get()) {
			final long now = getTransport().getLocalDevice().getClock().millis();
			try {
				final ByteQueue list = new ByteQueue();

				for (final FDTEntry e : foreignDeviceTable) {
					pushISA(list, e.address);
					list.pushU2B(e.timeToLive);

					int remaining = (int) (e.endTime - now) / 1000;
					if (remaining < 0)
						// Hasn't yet been cleaned up.
						remaining = 0;
					if (remaining > 65535)
						remaining = 65535;

					list.pushU2B(remaining);
				}

				// Successfully written.
				response.push(7); // Read-Foreign-Device-Table-Ack
				response.pushU2B(4 + list.size()); // Length
				response.push(list); // List
			} catch (final Exception e) {
				LOG.error("FDT read failed", e);
				response.push(0); // Result
				response.pushU2B(6); // Length
				response.pushU2B(0x40); // NAK
			}
		} else {
			response.push(0); // Result
			response.pushU2B(6); // Length
			response.pushU2B(0x40); // NAK
		}

		sendPacket(IpNetworkUtils.getInetSocketAddress(origin), response.popAll());
	}

	private void deleteFDT(final ByteQueue queue, final OctetString origin) throws BACnetException {
		final byte[] addr = new byte[4];
		queue.pop(addr);
		final int port = queue.popU2B();

		final ByteQueue response = new ByteQueue();
		response.push(BVLC_TYPE);
		response.push(0); // Response type
		response.pushU2B(6); // Length

		synchronized (foreignDeviceTable) {
			FDTEntry toDelete = null;
			for (final FDTEntry fd : foreignDeviceTable) {
				if (Arrays.equals(fd.address.getAddress().getAddress(), addr) && fd.address.getPort() == port) {
					toDelete = fd;
					break;
				}
			}

			if (toDelete != null) {
				foreignDeviceTable.remove(toDelete);
				response.pushU2B(0); // Success
			} else
				response.pushU2B(0x50); // NAK
		}

		sendPacket(IpNetworkUtils.getInetSocketAddress(origin), response.popAll());
	}

	private boolean distributeBroadcastToNetwork(final ByteQueue queue, final OctetString originStr)
			throws BACnetException {
		final InetSocketAddress origin = IpNetworkUtils.getInetSocketAddress(originStr);

		// Find the foreign device.
		FDTEntry originFDT = null;
		for (final FDTEntry fd : foreignDeviceTable) {
			if (fd.address.equals(origin)) {
				originFDT = fd;
				break;
			}
		}

		final ByteQueue response = new ByteQueue();
		response.push(BVLC_TYPE);
		response.push(0); // Response type
		response.pushU2B(6); // Length

		// If the FDT was not found, send a NAK and return false.
		if (originFDT == null) {
			response.pushU2B(0x60); // NAK
			sendPacket(origin, response.popAll());
			return false;
		}

		// The FDT was found. Forward the message around.
		final ByteQueue fwd = new ByteQueue();
		fwd.push(BVLC_TYPE);
		fwd.push(4); // Forward
		fwd.pushU2B(10 + queue.size()); // Length
		fwd.push(originStr.getBytes()); // Origin
		fwd.push(queue);
		final byte[] toSend = fwd.popAll();

		// Send locally
		sendPacket(InetAddrCache.get(broadcastAddressStr, port), toSend);

		try {
			// Send to all BDTs except own
			final byte[] myAddress = localBindAddress.getAddress().getAddress();
			for (final BDTEntry e : broadcastDistributionTable) {
				if (Arrays.equals(e.address, myAddress))
					continue;
				sendToBDT(e, toSend);
			}
		} catch (final UnknownHostException e1) {
			LOG.warn("Error forwarding to BDT", e1);
		}

		// Forward to all foreign devices except the origin.
		for (final FDTEntry fd : foreignDeviceTable) {
			if (fd != originFDT)
				sendPacket(fd.address, toSend);
		}

		response.pushU2B(0); // Success
		sendPacket(origin, response.popAll());
		return true;
	}

	private void sendToBDT(final BDTEntry e, final byte[] toSend) throws UnknownHostException, BACnetException {
		// J.4.5: The B/IP address to which the Forwarded-NPDU message is sent is formed by inverting the broadcast
		// distribution mask in the BDT entry and logically ORing it with the BBMD address of the same entry.
		final byte[] target = new byte[4];
		for (int i = 0; i < 4; i++)
			target[i] = (byte) (e.address[i] | ~e.distributionMask[i]);

		final InetSocketAddress to = InetAddrCache.get(InetAddress.getByAddress(target), e.port);
		sendPacket(to, toSend);
	}

	private static void pushISA(final ByteQueue queue, final InetSocketAddress isa) {
		queue.push(isa.getAddress().getAddress());
		queue.pushU2B(isa.getPort());
	}

	//
	//
	// Foreign device registration (of this local device in a foreign BBMD)
	//
	public void sendForeignDeviceRegistration() throws BACnetException {
		final InetSocketAddress addr = foreignBBMD;
		if (addr == null)
			return;

		final ByteQueue queue = new ByteQueue();
		queue.push(BVLC_TYPE);
		queue.push(0x05); // Register foreign device
		queue.pushU2B(6); // Length
		queue.pushU2B(foreignTTL); // TTL

		synchronized (addr) {
			bbmdResponse = -1;

			sendPacket(addr, queue.popAll());
			try {
				addr.wait(5000); // Wait up to 5 seconds.
			} catch (@SuppressWarnings("unused") final InterruptedException e) {
				// no op
			}

			if (bbmdResponse == -1)
				throw new BACnetException("Foreign registration timeout");
			if (bbmdResponse != 0)
				throw new BACnetException("NAK response: " + bbmdResponse);
		}
	}

	/**
	 * Utility method that allows the de-registration of an arbitrary foreign device in a BBMD. If the intention is
	 * to unregister this device, the unregisterAsForeignDevice method should be used instead.
	 *
	 * @param addr
	 * @param fdtEntry
	 * @throws BACnetException
	 */
	public void deleteForeignDeviceTableEntry(final InetSocketAddress addr, final InetSocketAddress fdtEntry)
			throws BACnetException {
		final ByteQueue queue = new ByteQueue();
		queue.push(BVLC_TYPE);
		queue.push(0x08); // Delete foreign device table entry
		queue.pushU2B(0xA); // Length
		pushISA(queue, fdtEntry);
		sendPacket(addr, queue.popAll());
	}

	/**
	 * Enable BBMD support. Allow other device to register as BBMD or foreign device. *
	 */
	public void enableBBMD() {
		bbmdEnabled.set(true);
	}

}
